//
// Created by jm on 2021/8/6.
//

#ifndef TWO_WHEEL_DIFFERENTIAL_INCLUDE_TWO_WHEEL_DIFFERENTIAL_H_
#define TWO_WHEEL_DIFFERENTIAL_INCLUDE_TWO_WHEEL_DIFFERENTIAL_H_
struct VelocityWheel {
  float left;
  float right;
};

struct VelocityVW {
  float v;
  float w;
};
struct VelocityWR {
  float w;
  float r;
};
struct VelocityVR {
  float v;
  float r;
};
struct VelocityVWR {
  float v;
  float w;
  float r;
};
static VelocityVWR Trans(const VelocityVW &v_vw) {
  if (v_vw.v == 0 && v_vw.w == 0)
    return {0, 0, 0};
  return {v_vw.v, v_vw.w, v_vw.v / v_vw.w};
}
static VelocityVWR TransWR2VW(const VelocityWR &v_vw) {
  return {v_vw.w * v_vw.r, v_vw.w, v_vw.r};
}

static VelocityVWR TransWheel2VWR(const VelocityWheel &v_rl, float length) {
  float v = (v_rl.left + v_rl.right) / 2;
  float w = (v_rl.right - v_rl.left) / length;
  float r = v/w;
  return {v,w,r};
}

static VelocityWR TransWheel2VR(const VelocityWheel &v_rl, float length) {
  return TransVW2WR(TransWheel2VW(v_rl, length));
}
static VelocityWheel TransVW2Wheel(const VelocityVW &v_vw, float length) {
  auto diff_v = v_vw.w * length;
  auto sum_v = v_vw.v * 2;
  return {(sum_v - diff_v) / 2, (sum_v + diff_v) / 2};
}
static VelocityWheel TransWR2Wheel(const VelocityVWR &v_vw, float length) {
  if (v_vw.` ==)
    return TransVW2Wheel(TransWR2VW(v_vw), length);
}

#endif //TWO_WHEEL_DIFFERENTIAL_INCLUDE_TWO_WHEEL_DIFFERENTIAL_H_
